#define __code
#include "pid.h"

double dt=0.1;
double T=0.0;
double C=17.0;
double G=0.08;

uint8_t func(uint16_t p)
{
    T += (p-G*T)/(C/dt+G/2);
    return 0;
}

PID_Param pp = {10000, 100, 6553, 0.1, &func};
PID_State ps = {0, 0, 0};

main(void)
{
    for(int i=0;i<1000;i++){
        int16_t err=(i-T)*100.0;
        PID_apply(&pp, &ps, err);
        printf("%d\n", err);
    }
    return 0;
}
